#include "sensor_electrode.h"

#define LEVEL1_BUFFER_SIZE                ( LEVEL2_SAMPLE_SECONDS / LEVEL1_SAMPLE_SECONDS )
#define LEVEL2_BUFFER_SIZE                  FIR_FILTER_LENGTH

static int32 (*m_pf_ReadElectrode[ELECTRODE_COUNT_CHANNELS])(void);
static boxcar m_t_boxcar[ELECTRODE_COUNT_CHANNELS] = {0};
static fixed32 m_f32_firBufData[ELECTRODE_COUNT_CHANNELS][LEVEL2_BUFFER_SIZE] = {0};
static cycle_buffer m_t_firBuf[ELECTRODE_COUNT_CHANNELS] = {0};
static int32 m_i32_interrupted = 0;


void Electrode_Initialize( int32 (*pf_ReadVCounter)(void),
                           int32 (*pf_ReadISig1)(void),
                           int32 (*pf_ReadISig2)(void) )
{
    uint32 i;
    for ( i=0; i<ELECTRODE_COUNT_CHANNELS; i++ )
    {
        Boxcar_Init( &m_t_boxcar[i], LEVEL1_BUFFER_SIZE );
        Buffer_Init( &m_t_firBuf[i], &m_f32_firBufData[i][0], LEVEL2_BUFFER_SIZE );
    }

    m_pf_ReadElectrode[ELECTRODE_C] = pf_ReadVCounter;
    m_pf_ReadElectrode[ELECTRODE_W1] = pf_ReadISig1;
    m_pf_ReadElectrode[ELECTRODE_W2] = pf_ReadISig2;
}


void Electrode_Read(void)
{    
    static fixed32 f32_last[ELECTRODE_COUNT_CHANNELS] = {0};
    uint32 i;
    fixed32 f32_value;

    for ( i=0; i<ELECTRODE_COUNT_CHANNELS; i++ )
    {
        if ( m_i32_interrupted > 0 )
            f32_value = f32_last[i];
        else
            f32_value = m_pf_ReadElectrode[i]();
        f32_last[i] = f32_value;
        Boxcar_Add( &m_t_boxcar[i], f32_value );
        if ( Boxcar_IsFull( &m_t_boxcar[i] ) )
        {
            f32_value = Boxcar_Output( &m_t_boxcar[i] );
            Buffer_Push( &m_t_firBuf[i], f32_value );
        }
    }
    if ( m_i32_interrupted > 0 )
        m_i32_interrupted--;
}

logical Electrode_Assert(void)
{
# define CURRENT_THRESHOLD_LOWER      1*PERCISION
# define CURRENT_THRESHOLD_UPPER    100*PERCISION
    return (Buffer_Get( &m_t_firBuf[ELECTRODE_W1], 0 ) >= CURRENT_THRESHOLD_LOWER || Buffer_Get( &m_t_firBuf[ELECTRODE_W1], 1 ) >= CURRENT_THRESHOLD_LOWER) &&
           (Buffer_Get( &m_t_firBuf[ELECTRODE_W1], 0 ) <  CURRENT_THRESHOLD_UPPER || Buffer_Get( &m_t_firBuf[ELECTRODE_W1], 1 ) <  CURRENT_THRESHOLD_UPPER) &&
           (Buffer_Get( &m_t_firBuf[ELECTRODE_W2], 0 ) <  CURRENT_THRESHOLD_UPPER || Buffer_Get( &m_t_firBuf[ELECTRODE_W2], 1 ) <  CURRENT_THRESHOLD_UPPER);
}

void Electrode_SetInterrupted(void)
{
    static logical b_first = L_TRUE;
    if (b_first)
    {
        m_i32_interrupted = LEVEL1_BUFFER_SIZE;
        b_first = L_FALSE;
    }
    else
    {
        m_i32_interrupted = BLANK_SECONDS_AFTER_EIS/LEVEL1_SAMPLE_SECONDS;//F_MIN( (int)(BLANK_SECONDS_AFTER_EIS/LEVEL1_SAMPLE_SECONDS), LEVEL1_BUFFER_SIZE );
    }

}

const cycle_buffer* Electrode_GetFirBuffer( electrode_channel t_channel )
{
    return &m_t_firBuf[t_channel];
}
